import Blender, yarp

from Blender.BGL import *

from middleware.yarp import YarpBlender

import array

#sensor = GameLogic.getCurrentController().getSensor('grab_image')
sensor = GameLogic.getCurrentController().getSensor('imageGrabbing')

#ok = sensor.isPositive() and sensor.isTriggered()
ok = sensor.isPositive()


#execute only when the 'grab_image' key is released (if we don't test that, the code get executed two time, when pressed, and when released)
if ok:
	#retrieve the YARP port we want to write on	
	p = YarpBlender.getPort('/blender_simu/cam')
	
	#retrieve the size and position of the robot camera, as stored in the initialization script
	v = GameLogic.camRobotViewport
		
	imX = v[2]
	imY = v[3]
	buf = Buffer(GL_BYTE,[imX*imY*3])
	
	#Call to the OpenGL library to copy the framebuffer to the main memory
	glReadPixels(370 + v[0], 340 + v[1],imX,imY,GL_RGB,GL_UNSIGNED_BYTE,buf)
	
	# Convert it to a form where we have access to a memory pointer
	data = array.array('B',buf.list)
	info = data.buffer_info()
	
	# Wrap the data in a YARP image
	img = yarp.ImageRgb()
	img.setTopIsLowIndex(0)
	img.setQuantum(1)
	img.setExternal(info[0],imX,imY)
	
	# copy to image with "regular" YARP pixel order
	img2 = yarp.ImageRgb()
	img2.copy(img)
	
	# Write the image
	p.write(img2) 
	
	#print "Wrote an image"


